#include "cellmath.h"

#include <iostream>
using namespace std;

CellMath::CellMath() { }

void CellMath::setEqual(double mat1[4][4], const double mat2[4][4])
{
    for(int i = 0; i < 4; i++) {
        for(int j = 0; i < 4; i++) {
            mat1[i][j] = mat2[i][j];
        }
    }
}

void CellMath::print4by4(const double mat[4][4])
{
    for (int i=0; i<4; i++) {
        for (int j=0; j<4; j++) {
            cout << mat[i][j] << " ";
        }
        cout << endl;
    }
}

bool CellMath::lineIntersection(const double a0, const double b0, const double a1, const double b1, const double a2, const double b2,
                                const double a, const double b, const double c, const double d, double &ix, double &iy, double &iz)
{
    double tmp = a*a0+b*a1+c*a2;
    if (tmp<CELL_EPS&&tmp>-CELL_EPS)
        return false;
    double t = (-d-a*b0-b*b1-c*b2)/tmp;
    ix = t*a0+b0;
    iy = t*a1+b1;
    iz = t*a2+b2;
    return true;
}

double CellMath::edgeIntersection(const double a0, const double b0, const double a1, const double b1, const double a2, const double b2,
                                const double a, const double b, const double c, const double d, double &ix, double &iy, double &iz)
{
    double tmp = a*a0+b*a1+c*a2;
    if (tmp<CELL_EPS&&tmp>-CELL_EPS)
        return -1.0;
    double t = (-d-a*b0-b*b1-c*b2)/tmp;
    ix = t*a0+b0;
    iy = t*a1+b1;
    iz = t*a2+b2;
    return t;
}

bool CellMath::triangleIntersection(const double a, const double b, const double c, const double d,
                                    const double vx0, const double vy0, const double vz0,
                                    const double vx1, const double vy1, const double vz1,
                                    const double vx2, const double vy2, const double vz2,
                                    double &rvx0, double &rvy0, double &rvz0,
                                    double &rvx1, double &rvy1, double &rvz1)
{
    double flag = edgeIntersection(vx0-vx1, vx1, vy0-vy1, vy1, vz0-vz1, vz1,
                                   a, b, c, d, rvx0, rvy0, rvz0);
    if (flag>=0.0&&flag<=1.0) {
        flag = edgeIntersection(vx0-vx2, vx2, vy0-vy2, vy2, vz0-vz2, vz2,
                                a, b, c, d, rvx1, rvy1, rvz1);
        if (flag>=0.0&&flag<=1.0)
            return true;
        else {
            flag = edgeIntersection(vx2-vx1, vx1, vy2-vy1, vy1, vz2-vz1, vz1,
                                    a, b, c, d, rvx1, rvy1, rvz1);
            if (flag>=0.0&&flag<=1.0)
                return true;
            else
                return false;
        }
    } else {
        flag = edgeIntersection(vx0-vx2, vx2, vy0-vy2, vy2, vz0-vz2, vz2,
                                a, b, c, d, rvx0, rvy0, rvz0);
        if (flag>=0.0&&flag<=1.0) {
            flag = edgeIntersection(vx2-vx1, vx1, vy2-vy1, vy1, vz2-vz1, vz1,
                                    a, b, c, d, rvx1, rvy1, rvz1);
            if (flag>=0.0&&flag<=1.0)
                return true;
            else
                return false;
        } else {
            return false;
        }
    }
}

double CellMath::lineIntersectionTriangle(const Vector3 pos, const Vector3 orientation, const Vector3 v0, const Vector3 v1, const Vector3 v2)
{
    Vector3 e1 = v1-v0;
    Vector3 e2 = v2-v0;
    Vector3 q = Cross(orientation,e2);
    double tmp = Dot(q,e1);
    if (tmp<CELL_EPS&&tmp>-CELL_EPS)
        return -1.0;
    tmp = 1.0f/tmp;
    Vector3 s = pos-v0;
    double u = tmp*Dot(s,q);
    if (u<0.0 || u>1.0)
        return -1.0;
    q = Cross(s,e1);
    double vv = tmp*Dot(orientation,q);
    if (vv<0.0 || vv+u>1.0)
        return -1.0;
    double dist = tmp*Dot(e2,q);
    if (dist<CELL_EPS)
        return -1.0;
    return dist;
}

void CellMath::resetMat(double mat[4][4])
{
    for (int i=0;i<4;i++) {
        for (int j=0;j<4;j++) {
            mat[i][j] = 0.0;
        }
    }
}

void CellMath::identityMat(double mat[4][4])
{
    for (int i=0;i<4;i++) {
        for (int j=0;j<4;j++) {
            if(i == j)
                mat[i][j] = 1.0;
            else
                mat[i][j] = 0.0;
        }
    }
}

void CellMath::rotateMatX(const double degree, double mat[4][4])
{
    //set all matrix values to 0
    resetMat(mat);
    //convert the degrees to radians
    double theta = degree*pi/180.0;
    //fill in the matrix
    mat[0][0] = 1.0;
    mat[1][1] = cos(theta);
    mat[1][2] = -sin(theta);
    mat[2][1] = sin(theta);
    mat[2][2] = cos(theta);
    mat[3][3] = 1.0;
}

void CellMath::rotateMatY(const double degree, double mat[4][4])
{
    resetMat(mat);
    double theta = degree*pi/180.0;
    mat[0][0] = cos(theta);
    mat[0][2] = sin(theta);
    mat[1][1] = 1.0;
    mat[2][0] = -sin(theta);
    mat[2][2] = cos(theta);
    mat[3][3] = 1.0;
}

void CellMath::rotateMatZ(const double degree, double mat[4][4])
{
    resetMat(mat);
    double theta = degree*pi/180.0;
    mat[0][0] = cos(theta);
    mat[0][1] = -sin(theta);
    mat[1][0] = sin(theta);
    mat[1][1] = cos(theta);
    mat[2][2] = 1.0;
    mat[3][3] = 1.0;
}

void CellMath::rotateMatFromAxis(const double degree, const double axisX, const double axisY, const double axisZ, double mat[4][4])
{
    resetMat(mat);

    double theta = degree*pi/180.0;
    double c = cos(theta);
    double s = sin(theta);
    mat[0][0] = c + pow(axisX, 2)*(1 - c);
    mat[0][1] = axisX*axisY*(1 - c) - axisZ*s;
    mat[0][2] = axisX*axisZ*(1 - c) + axisY*s;

    mat[1][0] = axisY*axisX*(1 - c) + axisZ*s;
    mat[1][1] = c + pow(axisY, 2)*(1 - c);
    mat[1][2] = axisY*axisZ*(1 - c) - axisX*s;

    mat[2][0] = axisZ*axisX*(1 - c) - axisY*s;
    mat[2][1] = axisZ*axisY*(1 - c) + axisX*s;
    mat[2][2] = c + pow(axisZ, 2)*(1 - c);

    mat[0][3] = mat[1][3] = mat[2][3] = mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 1.0;
}

void CellMath::translateMat(const double x, const double y, const double z, double mat[4][4])
{
    resetMat(mat);
    mat[0][3] = x;
    mat[1][3] = y;
    mat[2][3] = z;
    mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3]= 1.0;
}

void CellMath::scaleMat(const double x, const double y, const double z, double mat[4][4])
{
    resetMat(mat);
    mat[0][0] = x;
    mat[1][1] = y;
    mat[2][2] = z;
    mat[3][3] = 1.0;
}

void CellMath::matMutiplyVector(const double mat[4][4], const double v[4], double result[4])
{
    result[0] = mat[0][0]*v[0]+mat[0][1]*v[1]+mat[0][2]*v[2]+mat[0][3]*v[3];
    result[1] = mat[1][0]*v[0]+mat[1][1]*v[1]+mat[1][2]*v[2]+mat[1][3]*v[3];
    result[2] = mat[2][0]*v[0]+mat[2][1]*v[1]+mat[2][2]*v[2]+mat[2][3]*v[3];
    result[3] = mat[3][0]*v[0]+mat[3][1]*v[1]+mat[3][2]*v[2]+mat[3][3]*v[3];
}

void CellMath::matMutiply(const double mat1[4][4], const double mat2[4][4], double result[4][4])
{
    result[0][0] = mat1[0][0]*mat2[0][0] + mat1[0][1]*mat2[1][0] + mat1[0][2]*mat2[2][0] + mat1[0][3]*mat2[3][0];
    result[0][1] = mat1[0][0]*mat2[0][1] + mat1[0][1]*mat2[1][1] + mat1[0][2]*mat2[2][1] + mat1[0][3]*mat2[3][1];
    result[0][2] = mat1[0][0]*mat2[0][2] + mat1[0][1]*mat2[1][2] + mat1[0][2]*mat2[2][2] + mat1[0][3]*mat2[3][2];
    result[0][3] = mat1[0][0]*mat2[0][3] + mat1[0][1]*mat2[1][3] + mat1[0][2]*mat2[2][3] + mat1[0][3]*mat2[3][3];

    result[1][0] = mat1[1][0]*mat2[0][0]+mat1[1][1]*mat2[1][0]+mat1[1][2]*mat2[2][0]+mat1[1][3]*mat2[3][0];
    result[1][1] = mat1[1][0]*mat2[0][1]+mat1[1][1]*mat2[1][1]+mat1[1][2]*mat2[2][1]+mat1[1][3]*mat2[3][1];
    result[1][2] = mat1[1][0]*mat2[0][2]+mat1[1][1]*mat2[1][2]+mat1[1][2]*mat2[2][2]+mat1[1][3]*mat2[3][2];
    result[1][3] = mat1[1][0]*mat2[0][3]+mat1[1][1]*mat2[1][3]+mat1[1][2]*mat2[2][3]+mat1[1][3]*mat2[3][3];

    result[2][0] = mat1[2][0]*mat2[0][0]+mat1[2][1]*mat2[1][0]+mat1[2][2]*mat2[2][0]+mat1[2][3]*mat2[3][0];
    result[2][1] = mat1[2][0]*mat2[0][1]+mat1[2][1]*mat2[1][1]+mat1[2][2]*mat2[2][1]+mat1[2][3]*mat2[3][1];
    result[2][2] = mat1[2][0]*mat2[0][2]+mat1[2][1]*mat2[1][2]+mat1[2][2]*mat2[2][2]+mat1[2][3]*mat2[3][2];
    result[2][3] = mat1[2][0]*mat2[0][3]+mat1[2][1]*mat2[1][3]+mat1[2][2]*mat2[2][3]+mat1[2][3]*mat2[3][3];

    result[3][0] = mat1[3][0]*mat2[0][0]+mat1[3][1]*mat2[1][0]+mat1[3][2]*mat2[2][0]+mat1[3][3]*mat2[3][0];
    result[3][1] = mat1[3][0]*mat2[0][1]+mat1[3][1]*mat2[1][1]+mat1[3][2]*mat2[2][1]+mat1[3][3]*mat2[3][1];
    result[3][2] = mat1[3][0]*mat2[0][2]+mat1[3][1]*mat2[1][2]+mat1[3][2]*mat2[2][2]+mat1[3][3]*mat2[3][2];
    result[3][3] = mat1[3][0]*mat2[0][3]+mat1[3][1]*mat2[1][3]+mat1[3][2]*mat2[2][3]+mat1[3][3]*mat2[3][3];
}

void CellMath::InverseMat(const double mat[4][4], double result[4][4])
{
    float det = mat[0][0]*mat[1][1]*mat[2][2]*mat[3][3]+mat[0][0]*mat[1][2]*mat[2][3]*mat[3][1]+mat[0][0]*mat[1][3]*mat[2][1]*mat[3][2]+
            mat[0][1]*mat[1][0]*mat[2][3]*mat[3][2]+mat[0][1]*mat[1][2]*mat[2][0]*mat[3][3]+mat[0][1]*mat[1][3]*mat[2][2]*mat[3][0]+
            mat[0][2]*mat[1][0]*mat[2][1]*mat[3][3]+mat[0][2]*mat[1][1]*mat[2][3]*mat[3][0]+mat[0][2]*mat[1][3]*mat[2][0]*mat[3][1]+
            mat[0][3]*mat[1][0]*mat[2][2]*mat[3][1]+mat[0][3]*mat[1][1]*mat[2][0]*mat[3][2]+mat[0][3]*mat[1][2]*mat[2][1]*mat[3][0]-
            mat[0][0]*mat[1][1]*mat[2][3]*mat[3][2]-mat[0][0]*mat[1][2]*mat[2][1]*mat[3][3]-mat[0][0]*mat[1][3]*mat[2][2]*mat[3][1]-
            mat[0][1]*mat[1][0]*mat[2][2]*mat[3][3]-mat[0][1]*mat[1][2]*mat[2][3]*mat[3][0]-mat[0][1]*mat[1][3]*mat[2][0]*mat[3][2]-
            mat[0][2]*mat[1][0]*mat[2][3]*mat[3][1]-mat[0][2]*mat[1][1]*mat[2][0]*mat[3][3]-mat[0][2]*mat[1][3]*mat[2][1]*mat[3][0]-
            mat[0][3]*mat[1][0]*mat[2][1]*mat[3][2]-mat[0][3]*mat[1][1]*mat[2][2]*mat[3][0]-mat[0][3]*mat[1][2]*mat[2][0]*mat[3][1];
    if (det==0.0)
        return;

    result[0][0] = det*(mat[1][1]*mat[2][2]*mat[3][3]+mat[1][2]*mat[2][3]*mat[3][1]+mat[1][3]*mat[2][1]*mat[3][2]-
        mat[1][1]*mat[2][3]*mat[3][2]-mat[1][2]*mat[2][1]*mat[3][3]-mat[1][3]*mat[2][2]*mat[3][1]);
    result[0][1] = det*(mat[0][1]*mat[2][3]*mat[3][2]+mat[0][2]*mat[2][1]*mat[3][3]+mat[0][3]*mat[2][2]*mat[3][1]-
        mat[0][1]*mat[2][2]*mat[3][3]-mat[0][2]*mat[2][3]*mat[3][1]-mat[0][3]*mat[2][1]*mat[3][2]);
    result[0][2] = det*(mat[0][1]*mat[1][2]*mat[3][3]+mat[0][2]*mat[1][3]*mat[3][1]+mat[0][3]*mat[1][1]*mat[3][2]-
        mat[0][1]*mat[1][3]*mat[3][2]-mat[0][2]*mat[1][1]*mat[3][3]-mat[0][3]*mat[1][2]*mat[3][1]);
    result[0][3] = det*(mat[0][1]*mat[1][3]*mat[2][2]+mat[0][2]*mat[1][1]*mat[2][3]+mat[0][3]*mat[1][2]*mat[2][1]-
        mat[0][1]*mat[1][2]*mat[2][3]-mat[0][2]*mat[1][3]*mat[2][1]-mat[0][3]*mat[1][1]*mat[2][2]);

    result[1][0] = det*(mat[1][0]*mat[2][3]*mat[3][2]+mat[1][2]*mat[2][0]*mat[3][3]+mat[1][3]*mat[2][2]*mat[3][0]-
        mat[1][0]*mat[2][2]*mat[3][3]-mat[1][2]*mat[2][3]*mat[3][0]-mat[1][3]*mat[2][0]*mat[3][2]);
    result[1][1] = det*(mat[0][0]*mat[2][2]*mat[3][3]+mat[0][2]*mat[2][3]*mat[3][0]+mat[0][3]*mat[2][0]*mat[3][2]-
        mat[0][0]*mat[2][3]*mat[3][2]-mat[0][2]*mat[2][0]*mat[3][3]-mat[0][3]*mat[2][2]*mat[3][0]);
    result[1][2] = det*(mat[0][0]*mat[1][3]*mat[3][2]+mat[0][2]*mat[1][0]*mat[3][3]+mat[0][3]*mat[1][2]*mat[3][0]-
        mat[0][0]*mat[1][2]*mat[3][3]-mat[0][2]*mat[1][3]*mat[3][0]-mat[0][3]*mat[1][0]*mat[3][2]);
    result[1][3] = det*(mat[0][0]*mat[1][2]*mat[2][3]+mat[0][2]*mat[1][3]*mat[2][0]+mat[0][3]*mat[1][0]*mat[2][2]-
        mat[0][0]*mat[1][3]*mat[2][2]-mat[0][2]*mat[1][0]*mat[2][3]-mat[0][3]*mat[1][2]*mat[2][0]);

    result[2][0] = det*(mat[1][0]*mat[2][1]*mat[3][3]+mat[1][1]*mat[2][3]*mat[3][0]+mat[1][3]*mat[2][0]*mat[3][1]-
        mat[1][0]*mat[2][3]*mat[3][1]-mat[1][1]*mat[2][0]*mat[3][3]-mat[1][3]*mat[2][1]*mat[3][0]);
    result[2][1] = det*(mat[0][0]*mat[2][3]*mat[3][1]+mat[0][1]*mat[2][0]*mat[3][3]+mat[0][3]*mat[2][1]*mat[3][0]-
        mat[0][0]*mat[2][1]*mat[3][3]-mat[0][1]*mat[2][3]*mat[3][0]-mat[0][3]*mat[2][0]*mat[3][1]);
    result[2][2] = det*(mat[0][0]*mat[1][1]*mat[3][3]+mat[0][1]*mat[1][3]*mat[3][0]+mat[0][3]*mat[1][0]*mat[3][1]-
        mat[0][0]*mat[1][3]*mat[3][1]-mat[0][1]*mat[1][0]*mat[3][3]-mat[0][3]*mat[1][1]*mat[3][0]);
    result[2][3] = det*(mat[0][0]*mat[1][3]*mat[2][1]+mat[0][1]*mat[1][0]*mat[2][3]+mat[0][3]*mat[1][1]*mat[2][0]-
        mat[0][0]*mat[1][1]*mat[2][3]-mat[0][1]*mat[1][3]*mat[2][0]-mat[0][3]*mat[1][0]*mat[2][1]);

    result[3][0] = det*(mat[1][0]*mat[2][2]*mat[3][1]+mat[1][1]*mat[2][0]*mat[3][2]+mat[1][2]*mat[2][1]*mat[3][0]-
        mat[1][0]*mat[2][1]*mat[3][2]-mat[1][1]*mat[2][2]*mat[3][0]-mat[1][2]*mat[2][0]*mat[3][1]);
    result[3][1] = det*(mat[0][0]*mat[2][1]*mat[3][2]+mat[0][1]*mat[2][2]*mat[3][0]+mat[0][2]*mat[2][0]*mat[3][1]-
        mat[0][0]*mat[2][2]*mat[3][1]-mat[0][1]*mat[2][0]*mat[3][2]-mat[0][2]*mat[2][1]*mat[3][0]);
    result[3][2] = det*(mat[0][0]*mat[1][2]*mat[3][1]+mat[0][1]*mat[1][0]*mat[3][2]+mat[0][2]*mat[1][1]*mat[3][0]-
        mat[0][0]*mat[1][1]*mat[3][2]-mat[0][1]*mat[1][2]*mat[3][0]-mat[0][2]*mat[1][0]*mat[3][1]);
    result[3][3] = det*(mat[0][0]*mat[1][1]*mat[2][2]+mat[0][1]*mat[1][2]*mat[2][0]+mat[0][2]*mat[1][0]*mat[2][1]-
        mat[0][0]*mat[1][2]*mat[2][1]-mat[0][1]*mat[1][0]*mat[2][2]-mat[0][2]*mat[1][1]*mat[2][0]);
}

void CellMath::TranspoteMat(const double mat[4][4], double result[4][4])
{
    for (int i=0;i<4;i++)
        for (int j=0;j<4;j++)
            result[i][j] = mat[j][i];
}

double CellMath::crossAngle(const double ax, const double ay, const double bx, const double by)
{
    //return acos((ax*bx+ay*by)/(sqrt(ax*ax+ay*ay)*sqrt(bx*bx+by*by)))/pi*180.0;
    double at = atan2(ax, ay);
    double bt = atan2(bx, by);
    return (at-bt)/pi*180.0;
}

bool CellMath::isVectorInFrustum(const Vector3 p, const Vector3 point[8], const Vector3 view, const Vector3 up)
{
    if (Dot((point[0]-p),(-view))<0.0)
        return false;
    if (Dot((point[4]-p),view)<0.0)
        return false;
    Vector3 n = Cross((point[6]-point[2]),(point[6]-point[7]));
    if (Dot(n,up)<0.0)
        n=-n;
    if (Dot((point[2]-p),n)<0.0)
        return false;
    n = Cross((point[0]-point[1]),(point[0]-point[4]));
    if (Dot(n,(-up))<0.0)
        n = -n;
    if (Dot((point[0]-p),n)<0.0)
        return false;
    n = Cross((point[1]-point[2]),(point[1]-point[5]));
    if (Dot(n,Cross(view,up))>0.0)
        n = -n;
    if (Dot((point[1]-p),n)>0.0)
        return false;
    n = Cross((point[0]-point[3]),(point[0]-point[4]));
    if (Dot(n,(-Cross(view,up)))<0.0)
        n = -n;
    if (Dot((point[0]-p),n)<0.0)
        return false;
    return true;
}

void CellMath::crossProduct(const double x0, const double y0, const double z0, const double x1, const double y1, const double z1,
                         double &cpx, double &cpy, double &cpz)
{
    cpx = (y0*z1)-(z0*y1);
    cpy = (z0*x1)-(x0*z1);
    cpz = (x0*y1)-(y0*x1);
}

void CellMath::normalize(const double x, const double y, const double z, double &nx, double &ny, double &nz)
{
    double distance = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2));
    nx = x/distance;
    ny = y/distance;
    nz = z/distance;
}
